/* 
 * File: iRobotControl.c
 * Author: Tetsuya Idota and Brian Fehrman
 * Description: Contains function implementations for driving the iRobot
 * Created on (date): 2-5-2013
 */

#include "iRobotControl.h"

/////////////////////
// Function : DriveForward
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will cause the robot to drive forward by sending
// 			the same positive velocity value to each wheel.
/////////////////////
void DriveForward(uint16_t speed){
	DriveRL(speed,speed);
}

/////////////////////
// Function : DriveRL
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will send wheel velocity commands to
//			each wheel with units of mm/s.
//			Value is expected to be 2 bytes with
//			the high byte being sent first.
/////////////////////
void DriveRL( int16_t rspeed, int16_t lspeed ){
	usart1tx((uint8_t)145);
	usart1tx((uint8_t)(rspeed >> 8));
	usart1tx((uint8_t)(rspeed));
	usart1tx((uint8_t)(lspeed >> 8));
	usart1tx((uint8_t)(lspeed));
}

/////////////////////
// Function : DriveStop
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Stops the robot by sending 0,0 to the DriveRL function
/////////////////////
void DriveStop(){
	DriveRL(0,0);
}

/////////////////////
// Function : DriveReverse
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will cause the robot to drive in reverse by sending
// 			the same negative velocity value to each wheel.
/////////////////////
void DriveReverse(uint16_t speed){
	DriveRL(-1*speed,-1*speed);
}

/////////////////////
// Function : Rotate180
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will cause the robot to rotate 180 by sending the same
//			velocity value with a different sign to each wheel. The
//			"wait angle" OpCode is then sent to the robot and it is
//			commanded to turn until 180 degrees of angular displacement
//			has been reached. This version of the function also takes a speed
/////////////////////
void Rotate180( uint16_t speed ){
	DriveRL(speed,-speed);
	usart1tx((uint8_t)157);
	usart1tx((uint8_t)0);
	usart1tx((uint8_t)180);
	DriveStop();
}

/////////////////////
// Function : DriveDist
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will cause the robot to drive forward at a given speed
//			for a specified distance. The DriveRL command is called,
//			then the "wait distance" OpCode is sent to the robot
//			followed by the desired distance (2 bytes, high byte first)
/////////////////////
void DriveDist(uint16_t speed, uint16_t dist){
	DriveRL(speed,speed);
	usart1tx((uint8_t)156);
	usart1tx((uint8_t)(dist>>8));
	usart1tx((uint8_t)(dist));
	DriveStop();
}


/////////////////////
// Function : TurnAngle
// Authors: Tetsuya Idota and Brian Fehrman
//
// Purpose: Will cause the robot to rotate a given angle by sending the same
//			velocity value with a different sign to each wheel. The
//			"wait angle" OpCode is then sent to the robot and it is
//			commanded to turn until the desired angular displacement
//			has been reached. This version of the function takes speed
/////////////////////
void TurnAngle(int16_t angle, uint16_t speed){
	
	if(angle > 0)
		DriveRL(speed,-speed);
	else
		DriveRL(-speed, speed);
		
	usart1tx((uint8_t)157);
	usart1tx((uint8_t)(angle>>8));
	usart1tx((uint8_t)angle);
	DriveStop();
}
